#include "CybertronBridgeIO.hpp"
#include "Util/BridgeIOConfig.hpp"

// Tasks

#include "Traffic/ScenarioEvent.pb.h"
#include "Traffic/TrafficCommon.pb.h"
#include "Traffic/TrafficEvent.pb.h"

#include "cybertron/glm/quat.hpp"

#include "Util/UtilString.hpp"
#include "Task/HotAreaTask/TaskHotAreaBase.hpp"
#include <fstream>
#include <thread>
#include <stdlib.h>
#include "SimOneIOStruct.h"

#ifdef CYBERTRON_WIN
#include "Simulink/SimulinkFromProj.hpp"
#include "Simulink/SimulinkLinkInstance.hpp"
#endif

CYBERTRON_BEGIN

CybertronBridgeIO::CybertronBridgeIO()
{
	mbGetHotArea = false;
	mIsCanOnTimering = false;
	//mpBridgeIOServer = pServer;
}

CybertronBridgeIO::~CybertronBridgeIO()
{
#ifdef CYBERTRON_WIN
	delete mSimulinkInterface;
#endif
}
void CybertronBridgeIO::runServer()
{
	if (!mpBridgeIOServer->initialize())
	{
		logOutputError("", "Failed to initialize server!");
		return;
	}
	mpBridgeIOServer->listen(23789);
	logInfo("CybertronBridgeIO Port:%d", 23789);
	setServerListenPort(23789);
	mpBridgeIOServer->run();
	mpBridgeIOServer->close();
	mpBridgeIOServer = nullptr;
}

bool CybertronBridgeIO::init(int argc, char* argv[])
{
	WorkAppWithHotArea::init(argc, argv);
	mpBridgeIOServer = std::make_shared<BridgeIOServer>();
	std::thread thread(&CybertronBridgeIO::runServer, this);
	thread.detach();
	return true;
}

void CybertronBridgeIO::getVerion()
{
	//std::string version = getVersionOnCore("CybertronBridgeIO", "20210517111111");
	//logInfo("%s", version.c_str());
}


void setLogServerConfig() {
	//Read environment variables of logServer
	char *logServerIp = theStartupArguments().getParamValue("LOG_SERVER_IP");//getenv("LOG_SERVER_IP");
	if (logServerIp != nullptr) {
		//int logServerPort = atoi(getenv("LOG_SERVER_PORT"));
		int logServerPort = atoi(theStartupArguments().getParamValue("LOG_SERVER_PORT"));
		logServer::setLogServerAddress(logServerIp, logServerPort);
		logOutputInfo("", "Read environment variables logServerIP:%s logServerPort:%d", logServerIp, logServerPort);
		return;
}

	//Read configuration file of logServer
	cybertron::json jsonSensor;
	if (!JsonReader::loadFile(jsonSensor, "logOutputConfig.json"))
	{
		logOutputInfo("", "Failed to load logOutput Config json file,use default value");
		logServer::setLogServerAddress();
		return;
	}
	else
	{
		std::string logServerIP = JsonReader::getString(jsonSensor, "LOG_SERVER_IP");
		if (logServerIP.empty())
		{
			logOutputWarn("", "LOG_SERVER_IP is empty");
			return;
		}
		int logServerPort = atoi(JsonReader::getString(jsonSensor, "LOG_SERVER_PORT").c_str());
		logOutputInfo("", "Read configuration file logServerIP:%s logServerPort:%d", logServerIP.c_str(), logServerPort);
		logServer::setLogServerAddress(logServerIP.c_str(), logServerPort);
	}
}

#ifdef CYBERTRON_LINUX
void CybertronBridgeIO::startScriptWithSsh(const std::string& script)
{
	std::vector<std::string> params;
	std::string cmd = "ssh";
	std::vector<std::string> envs;
	std::string startScriptDir = "~";
	char* mHostIP = theStartupArguments().getParamValue("CZ_WEB_DOWNLOAD_HOST");//std::getenv("CZ_WEB_DOWNLOAD_HOST");
	char* mHostUserName = theStartupArguments().getParamValue("HOST_USER_NAME");//std::getenv("HOST_USER_NAME");
	if (mHostIP == ""){
		logOutputWarn(BridgeConfig::getInstance().getTaskId().c_str(),"CZ_WEB_DOWNLOAD_HOST is not declared.");
	}
	if (mHostIP == "127.0.0.1") {
		logOutputWarn(BridgeConfig::getInstance().getTaskId().c_str(), "please set your environment: CZ_WEB_DOWNLOAD_HOST.");
	}
	params.push_back("-l");
	params.push_back(mHostUserName);
	params.push_back(mHostIP);
	params.push_back(script);
	if (!cybertron::UtilProcess::start(
		cmd,
		startScriptDir,
		params,
		mStartProcessHandle, envs)){
			logOutputWarn(BridgeConfig::getInstance().getTaskId().c_str(), "Can not start script with ssh.IP=%s, script is:%s", mHostIP, mStartScriptPath.c_str());
	}
}
#endif

bool CybertronBridgeIO::onLoadConfig() {
	if (!CybertronBridgeBase::onLoadConfig()) {
		return false;
	}
	BridgeConfig &config = BridgeConfig::getInstance();
	Agent::WorkNodeRegisterRet* pWorkNodeRegisterRetData = getWorkNodeRegisterRetData();

	config.loadConfig(pWorkNodeRegisterRetData, this);
	return true;
}
bool CybertronBridgeIO::onStartBridge()
{
    //// Read config
	setSyncBridge(true);
	BridgeConfig &config = BridgeConfig::getInstance();
	
#ifdef CYBERTRON_WIN
	mMatlabEnable = config.runMode == "auto" ? true : false;
#endif
	bool isUseDefinedPath = config.simulinkType == "local";
	std::string slxPath = config.simulinkPath;
	auto hostVehicleConfig = config.getHostVehicleConfig();
	TaskHotAreaManager::getInstance().Init(this);
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		const auto sensorConfig = config.sensorGpsImuConfig(item->first);
		mIMUNoiseMaker = std::make_shared<IMUNoiseMaker>(sensorConfig);
		mStartScriptPath = config.getStartScriptPath(item->first);
		mEndScriptPath = config.getEndScriptPath(item->first);
#ifdef CYBERTRON_WIN
		if (mStartScriptPath != "") {
			logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(), "try to start algorithm,script is: %s", mStartScriptPath.c_str());
			WinExec(mStartScriptPath.c_str(), SW_NORMAL);
		}
#endif
#ifdef CYBERTRON_LINUX
		if (mStartScriptPath != "") {
			startScriptWithSsh(mStartScriptPath);
		}
#endif
		std::vector<BridgeConfig::NodeInstance> *nodes = config.nodes(item->first);
		
		
		/* Add bridge tasks */
		auto vehicleId = item->first;
		for (auto it = nodes->begin(); it != nodes->end(); it++) {// vehicle node is always the first node
			if (it->type == ESimOne_Node_Type_Vehicle && it->valid) {
				mVehicleId = std::atoi(vehicleId.c_str());
				TaskHotAreaManager::getInstance().AddMainVehicleID(mVehicleId);
				if (item->second.bIsCustomModule) {
					mpTaskControlModuleRun = std::make_shared<TaskControlModuleRun>(vehicleId);
					addBridgeTask(mpTaskControlModuleRun);
				}
			}
		}

		if (config.GetSimOneAPITotalCount(vehicleId) > 0) {
			int count = 0;
			while (config.IsWaitSimOneAPI(vehicleId))
			{
				if (count > 5) {
					break;
				}
				std::this_thread::sleep_for(std::chrono::milliseconds(1000));
				logInfo("vehicle id:%s wait simone api connect........", vehicleId.c_str());
				count++;
			}
		}
#ifdef CYBERTRON_WIN

		if (mMatlabEnable) {
			bool isSetPath = isUseDefinedPath; // isUseDefinedPath
			if (isSetPath) {
				mSimulinkInterface = new SimulinkFromProj(slxPath);
			}
			else {
				mSimulinkInterface = new SimulinkLinkInstance();
			}
			mSimulinkInterface->init();
		}
#endif

	}
	return true;
}

void CybertronBridgeIO::onMessageCommand(std::uint32_t msgId, const std::string * msgDataBody)
{
	if (msgId == cybertron::proto::traffic::EScenarioEvent){
		cybertron::proto::traffic::ScenarioEvent evt;
		if (evt.ParseFromString(*msgDataBody)){
			if (evt.has_triggerevent()){
				int targetId = 0;
				auto& triggerEvt = evt.triggerevent();
				if (triggerEvt.target_size() > 0){
					targetId = atoi(triggerEvt.target(0).c_str());
				}
				logInfo( "scenario command frame:%d event:%s data:%s", mFrame, triggerEvt.event().c_str(),triggerEvt.data().c_str());
				mpBridgeIOServer->SendScenarioEvent(targetId, msgDataBody);
			}
		}

	}
	else if (msgId == cybertron::proto::traffic::ETrafficEvent) {
		cybertron::proto::traffic::TrafficEvent event;
		if (event.ParseFromString(*msgDataBody)) {
			logInfo("traffic command frame:%d event:%s vehicleid:%d data:%s", mFrame, event.taskuuid().c_str(), event.vehicleid(), event.data().c_str());
			mpBridgeIOServer->SendTrafficEvent(msgDataBody);
		}
	}
	else if (msgId == cybertron::proto::traffic::ECollisionFromOnEnd) {
		int reason = 2;//collision events are logged only
		logInfo("collision events are logged only:%d frame:%d", reason, mFrame);
		mpBridgeIOServer->SendEndTaskReason(mFrame, reason);
	}
}	
bool CybertronBridgeIO::onTaskStartAfter() {
	std::vector<std::string> workIdList;
	getWorkIdListByType(Common::EWorkNodeType_CybertronVehicleDynamic, workIdList);
	if (workIdList.size() != 0) {
		CybertronSyncConn client;
		for (auto i = 0; i < workIdList.size(); i++) {
			bool bIsConnect = connectWorkServer(workIdList[i], client);
			if (bIsConnect) {
				auto hostVehicleId = getHostVehicleIdByWorkId(workIdList[i]);
				mpBridgeIOServer->AddConnHostVehicleClient(hostVehicleId, bIsConnect, client);
			}
		}
	}
	return true;
}
void CybertronBridgeIO::onTimer(int framestamp, int ms, int msModNs) {
	mFrame = framestamp;
	mpBridgeIOServer->SendFrameStart(framestamp);
	TaskHotAreaManager::getInstance().HotAreaOnFrame(framestamp, ms, msModNs);
	mpBridgeIOServer->SendFrameEnd(framestamp);
	mpBridgeIOServer->FrameStep(framestamp);
	//CybertronBridgeBase::onTimer( framestamp,  ms,  msModNs);
}

void CybertronBridgeIO::onCaseStart() {
	logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(), "onCaseStart================>");
	BridgeConfig::getInstance().SetCaseStatus(2);
	mpBridgeIOServer->SendCaseStart();
	//config.SetCaseStart();
#ifdef CYBERTRON_WIN
    if (mMatlabEnable) {
        mSimulinkInterface->startSim();
    }
#endif
}
void CybertronBridgeIO::onCaseStop() {
	logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(), "onCaseStop================>");
	if (mpTaskControlModuleRun) {
		cybertron::ProcessHandle handle = mpTaskControlModuleRun->getProcessHandle();
		if (handle.isValid()) {
			logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(), "kill prcoess================>");
			cybertron::UtilProcess::terminateAndCloseHandle(handle);
		}
	}
	BridgeConfig &config = BridgeConfig::getInstance();
	auto hostVehicleConfig = config.getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		mEndScriptPath = config.getEndScriptPath(item->first);
#ifdef CYBERTRON_WIN
		if (mEndScriptPath != "") {
			logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(), "try to stop algorithm,script is: %s", mEndScriptPath.c_str());
			WinExec(mEndScriptPath.c_str(), SW_NORMAL);
		}
#endif
#ifdef CYBERTRON_LINUX
		if (mEndScriptPath != "") {
			startScriptWithSsh(mEndScriptPath);
		}
#endif
	}
	BridgeConfig::getInstance().SetCaseStatus(1);
	mpBridgeIOServer->SendCaseEnd();
}
void CybertronBridgeIO::onPlay() {
	//SimOne_Case_Status_Unknow = 0,
//	SimOne_Case_Status_Stop = 1,
//	ESimOne_Case_Status_Running = 2,
//	SimOne_Case_Status_Pause = 3
	BridgeConfig::getInstance().SetCaseStatus(ESimOne_Case_Status_Running);
	mpBridgeIOServer->sendCaseStatusChange(ESimOne_Case_Status_Running);
	logInfo("onPlay================>");
#ifdef CYBERTRON_WIN
	if (mMatlabEnable) {
        mSimulinkInterface->continueSim();
	}
#endif
}
void CybertronBridgeIO::onPause() {
	logInfo("onPause================>");
#ifdef CYBERTRON_WIN
	if (mMatlabEnable) {
        mSimulinkInterface->pauseSim();
	}
#endif
	BridgeConfig::getInstance().SetCaseStatus(ESimOne_Case_Status_Pause);
	mpBridgeIOServer->sendCaseStatusChange(ESimOne_Case_Status_Pause);
}
void CybertronBridgeIO::onReset() {
	logOutputInfo(BridgeConfig::getInstance().getTaskId().c_str(),"================>onReset");
}

void CybertronBridgeIO::onEnd() {
#ifdef CYBERTRON_WIN
	if (mMatlabEnable) {
        mSimulinkInterface->endSim();
	}
#endif
	logInfo("================>onEnd");

}
bool  CybertronBridgeIO::onTaskStop() {
	int reason = mTaskStopFromeAgent.reason();
	logInfo("===onTaskStop reason:%d", mTaskStopFromeAgent.reason());
	mpBridgeIOServer->SendEndTaskReason(mFrame, reason);
	return true;
}
CYBERTRON_END
